#ifndef VEHICLEJOGCONTROL_H
#define VEHICLEJOGCONTROL_H

#include "vehiclekinematics.h"
#include "vehicledefine.h"
#include <QString>
#include <vector>

//struct VehicleVelocity
//{
//    double x_move;//according to vehicle unit in m/s
//    double y_move;
//    double z_rotate;//unit is rad/s
//};

//struct VehiclePosition
//{
//    double x;//m
//    double y;//m
//    double rotate;//unit is rad
//};

class VehicleJogControl
{
public:
    VehicleJogControl(QString fileName);

public:
    int stop();
    int startRun();
    bool isInitialOk();
    int vehicleJog(double xMove, double yMove, double rotate);
    int vehicleAuto(double xMove, double yMove, double rotate);
    int getVehicleVelCommand(double &xMove, double &yMove, double &rotate);
    int moveLoop(QVector<double> &wheelVel);
    int stopVel(QVector<double> &wheelVel);
    int getWheelData(std::vector<double>& wheelMoveSpeed,std::vector<double>& wheelPathLength);
    int getOdometer(VehicleVelocity& vehicleVelFeedbackOut,
                    VehiclePosition& vehiclePositionFeedbackOut);
    int odometerCalculationLoop(QVector<double> wheelVelFeedback, QVector<double> wheelPosFeedback);

private:
    int recordWheelFeedback(QVector<double> wheelVelFeedbackIn, QVector<double> wheelPosFeedbackIn);
    int readConfigFile(QString fileName);
    int speedLimitCheck(QVector<double> wheelVel);
    double getTimeInterval(struct timespec& lastTime);//return S

private:
    bool initialFlag;
    E_VEHICLE_MODE mode;
    E_VEHICLE_TYPE vehicletype;
    VehicleVelocity vehicleVelJogCommand;
    VehicleVelocity vehicleVelAutoCommand;
    VehicleVelocity vehicleMaxVel;
    double wheelDiameter;
    double wheelDistance;
    double wheelMaxSpeed;

    VehicleKinematics* vehicleKinematic;
    E_VEHICLE_STATUS status;

    VehicleVelocity vehicleVelFeedback;
    VehiclePosition vehiclePositionFeedback;
    struct timespec timeStamp;
    bool timeStampInitialed;

    double wheelVelFeedback[100];
    double wheelPosFeedback[100];

};

#endif // VEHICLEJOGCONTROL_H
